using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Threading;

namespace Dida
{
	public class SpatialHash
	{
		float cellSize;
		float reciCellSize;

		Dictionary<CellPosition, Cell> cells = new Dictionary<CellPosition, Cell>();

		public SpatialHash()
			: this(5)
		{
		}

		public SpatialHash(float cellSize)
		{
			this.cellSize = cellSize;
			reciCellSize = 1 / cellSize;
		}

		public void AddObject(ISpatialHashObject obj)
		{
			Debug.Assert(obj.HashObjState == null,
				"You can't add an object to multiple SpatialHash'es");

			HashObjState state = new HashObjState();
			obj.HashObjState = state;
			state.obj = obj;
			state.prevMinX = int.MaxValue;
			state.prevMinY = int.MaxValue;
			state.prevMinZ = int.MaxValue;
			state.prevMaxX = int.MinValue;
			state.prevMaxY = int.MinValue;
			state.prevMaxZ = int.MinValue;
			UpdateObject(obj);
		}

		public void RemoveObject(ISpatialHashObject obj)
		{
			HashObjState state = (HashObjState)obj.HashObjState;
			if (state == null)
				return;

			for (int z = state.prevMinZ; z <= state.prevMaxZ; z++)
			{
				for (int y = state.prevMinY; y <= state.prevMaxY; y++)
				{
					for (int x = state.prevMinX; x <= state.prevMaxX; x++)
					{
						CellPosition cellPos = new CellPosition(x, y, z);
						Cell cell = cells[cellPos];
						cell.objs.Remove(state);
						if (cell.objs.Count == 0)
							cells.Remove(cellPos);
					}
				}
			}

			obj.HashObjState = null;
		}

		public void UpdateObject(ISpatialHashObject obj)
		{
			HashObjState state = (HashObjState)obj.HashObjState;

			Box bbox = obj.BoundingBox;
			int newMinX = (int)Math.Floor(bbox.min.x * reciCellSize);
			int newMinY = (int)Math.Floor(bbox.min.y * reciCellSize);
			int newMinZ = (int)Math.Floor(bbox.min.z * reciCellSize);

			int newMaxX = (int)Math.Ceiling(bbox.max.x * reciCellSize);
			int newMaxY = (int)Math.Ceiling(bbox.max.y * reciCellSize);
			int newMaxZ = (int)Math.Ceiling(bbox.max.z * reciCellSize);

			// skip objects that didn't move at all.
			if (newMinX == state.prevMinX &&
				newMinY == state.prevMinY &&
				newMinZ == state.prevMinZ &&
				newMaxX == state.prevMaxX &&
				newMaxY == state.prevMaxY &&
				newMaxZ == state.prevMaxZ)
			{
				return;
			}
			
			int loopMinX = Math.Min(newMinX,state.prevMinX);
			int loopMinY = Math.Min(newMinY,state.prevMinY);
			int loopMinZ = Math.Min(newMinZ,state.prevMinZ);
			int loopMaxX = Math.Max(newMaxX,state.prevMaxX);
			int loopMaxY = Math.Max(newMaxY,state.prevMaxY);
			int loopMaxZ = Math.Max(newMaxZ,state.prevMaxZ);
			
			// The idea is to just remove the object from all cells it is currently added
			// to, and then readd it the new cells.
			// An object should be removed if the removeZ, removeY and removeZ flags are true, 
			// and should be added when the addZ, addY and addX are true. To safe performance
			// we just leave it the way it is, when both the add and remove flags are true.
			for (int z = loopMinZ; z <= loopMaxZ; z++)
			{
				bool addZ = z >= newMinZ && z <= newMaxZ;
				bool removeZ = z >= state.prevMinZ && z <= state.prevMaxZ;
					
				for (int y = loopMinY; y <= loopMaxY; y++)
				{
					bool addY = y >= newMinY && y <= newMaxY;
					bool removeY = y >= state.prevMinY && y <= state.prevMaxY;

					for (int x = loopMinX; x <= loopMaxX; x++)
					{
						bool addX = x >= newMinX && x <= newMaxX;
						bool removeX = x >= state.prevMinX && x <= state.prevMaxX;

						bool shouldAdd = addX && addY && addZ;
						bool shouldRemove = removeX && removeY && removeZ;

						if (shouldAdd && !shouldRemove)
						{
							// add the cell

							CellPosition cellPos = new CellPosition(x, y, z);
							
							Cell cell;
							if (!cells.TryGetValue(cellPos, out cell))
							{
								cell = new Cell();
								cell.objs = new HashSet<HashObjState>();
								cells.Add(cellPos, cell);
							}
							cell.objs.Add(state);
						}
						else if (shouldRemove && !shouldAdd)
						{
							// remove the cell

							CellPosition cellPos = new CellPosition(x, y, z);
							
							Cell cell = cells[cellPos];
							cell.objs.Remove(state);
							if(cell.objs.Count == 0)
								cells.Remove(cellPos);
						}
					}
				}
			}

			state.prevMinX = newMinX;
			state.prevMinY = newMinY;
			state.prevMinZ = newMinZ;
			state.prevMaxX = newMaxX;
			state.prevMaxY = newMaxY;
			state.prevMaxZ = newMaxZ;			
		}

		public SpatialHashCache GetLocalObjects(Box box)
		{
			int minX = (int)Math.Floor(box.min.x * reciCellSize);
			int minY = (int)Math.Floor(box.min.y * reciCellSize);
			int minZ = (int)Math.Floor(box.min.z * reciCellSize);

			int maxX = (int)Math.Ceiling(box.max.x * reciCellSize);
			int maxY = (int)Math.Ceiling(box.max.y * reciCellSize);
			int maxZ = (int)Math.Ceiling(box.max.z * reciCellSize);

			SpatialHashCache ret = new SpatialHashCache();

			for (int z = minZ; z <= maxZ; z++)
			{
				for (int y = minY; y <= minY; y++)
				{
					for (int x = minX; x <= maxX; x++)
					{
						CellPosition cellPos = new CellPosition(x, y, z);
						Cell cell;
						if (cells.TryGetValue(cellPos, out cell))
						{
							ret.closeCells.Add(cell);
						}
					}
				}
			}

			return ret;
		}

		public SpatialHashCache GetLocalObjects(Sphere sphere)
		{
			return GetLocalObjects(sphere.BoundingBox);
		}

		internal struct CellPosition
		{
			public int x;
			public int y;
			public int z;

			public CellPosition(int x, int y, int z)
			{
				this.x = x;
				this.y = y;
				this.z = z;
			}

			public override int GetHashCode()
			{
				unchecked
				{
					uint ret = (uint)2166136261;
					ret = (uint)(ret ^ x) * (uint)16777619;
					ret = (uint)(ret ^ y) * (uint)16777619;
					ret = (uint)(ret ^ z) * (uint)16777619;
					return (int)ret;
				}
			}

			public override bool Equals(object obj)
			{
				CellPosition other = (CellPosition)obj;
				return x == other.x && y == other.y && z == other.z;
			}
		}

		internal class Cell
		{
			public HashSet<HashObjState> objs;
		}

		internal class HashObjState
		{
			public ISpatialHashObject obj;

			public int prevMinX;
			public int prevMinY;
			public int prevMinZ;
			public int prevMaxX;
			public int prevMaxY;
			public int prevMaxZ;
		}
	}

	public interface ISpatialHashObject
	{
		Box BoundingBox
		{
			get;
		}

		object HashObjState
		{
			get;
			set;
		}
	}

	public class SpatialHashCache : IEnumerable<ISpatialHashObject>
	{
		internal List<SpatialHash.Cell> closeCells = new List<SpatialHash.Cell>();

		internal SpatialHashCache() { }

		public IEnumerator<ISpatialHashObject> GetEnumerator()
		{
			return new Enumerator(this);
		}		

		System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator()
		{
			return new Enumerator(this);
		}

		private class Enumerator : IEnumerator<ISpatialHashObject>
		{
			SpatialHashCache cache;

			int curCell = -1;
			IEnumerator<SpatialHash.HashObjState> curObj;

			public Enumerator(SpatialHashCache cache)
			{
				this.cache = cache;
			}

			public ISpatialHashObject Current
			{
				get { return curObj.Current.obj; }
			}

			public void Dispose() { }

			object System.Collections.IEnumerator.Current
			{
				get { return curObj.Current; }
			}

			public bool MoveNext()
			{
				if(curCell == -1)
				{
					curCell = 0;
					curObj = cache.closeCells[curCell].objs.GetEnumerator();
				}

				while (!curObj.MoveNext())
				{
					curCell++;
					if (curCell >= cache.closeCells.Count)
						return false;
				}

				return true;
			}

			public void Reset()
			{
				curCell = -1;
			}
		}
	}
}
